#include "adafruit_feather_wing/adafruit_feather_wing.h"

diffbot::AdafruitMotorController::AdafruitMotorController(uint8_t motor_num, uint8_t addr)
{
    motor_driver_ = Adafruit_MotorShield(addr);
    // 选择哪个“端口”（电机引脚）M1, M2, M3 或 M4。
    pMotor_ = motor_driver_.getMotor(motor_num);
}

void diffbot::AdafruitMotorController::begin(uint16_t freq)
{
    motor_driver_.begin(freq);
}

void diffbot::AdafruitMotorController::setSpeed(int value)
{
    if (value > 0)
    {
        pMotor_->run(FORWARD);
    }
    else if (value < 0)
    {
        pMotor_->run(BACKWARD);
        // Adafruit_MotorShield 需要 0 到 255 的 uint8 值
        // 确保将负值转换为正值
        value = value * -1;
    }
    else // 零速度
    {
        // 切断电机的电源
        pMotor_->run(RELEASE);
    }

    pMotor_->setSpeed(value);
}